
#class auto

// TCP-IP CONFIGURATION
       #define TCPCONFIG 1  //STATIC IP
//       #define _PRIMARY_STATIC_IP 	"192.168.0.200"
//       #define _PRIMARY_NETMASK 		"255.255.255.0"
//       #define MY_GATEWAY				"192.168.0.1"

//	      #define TCPCONFIG 5  //DHCP
         #define WIFI_USE_WPA
         #define IFC_WIFI_SSID         "pie"
//			#define IFC_WIFI_SSID			  "robotAdhoc"
         #define _PRIMARY_STATIC_IP      "192.168.1.201"
         #define _PRIMARY_NETMASK        "255.255.255.0"
         #define MY_GATEWAY              "192.168.1.1"
         #define MY_NAMESERVER           "192.168.1.1"
			#define IFC_WIFI_ROAM_ENABLE  1
			#define IFC_WIFI_ROAM_BEACON_MISS  20
         #define IFC_WIFI_MODE         IFPARAM_WIFI_INFRASTRUCTURE
//         #define IFC_WIFI_CHANNEL        1
//			#define IFC_WIFI_MODE				IFPARAM_WIFI_ADHOC
         #define IFC_WIFI_REGION       IFPARAM_WIFI_REGION_AMERICAS
//         #define IFC_WIFI_ENCRYPTION   IFPARAM_WIFI_ENCR_NONE
         #define IFC_WIFI_ENCRYPTION   IFPARAM_WIFI_ENCR_TKIP
			#define IFC_WIFI_WPA_PSK_HEXSTR "69731fd54810a9264ba07ee71205cf08fd38c96a3bd7eb8ef463ccacd488ac9b"  //pie
//			#define IFC_WIFI_WPA_PSK_HEXSTR "7c0359945c6b03056bc81aa3c7552404871c1c7eeba36713857e7191f1f246a9"  //razael

                                         //robotics
         //http://www.xs4all.nl/~rjoris/wpapsk.html
         //^ WPA PSK hex string from passphrase calculator

//TCPCONFIG

#define MAX_UDP_SOCKET_BUFFERS 2
#define INTERFACE_PORT	1300
#define FIELD_PORT			5000

//Define REMOTE_IP to restrict socket access to a single IP
//Set to "0" to set to "First connector to socket" (Default)

#define REMOTE_IP			"0"
#define FIELD_IP			"192.168.1.50"
//#define REMOTE_IP			"192.168.0.54"



#define CINBUFSIZE  15
#define COUTBUFSIZE 15
#define DINBUFSIZE  15
#define DOUTBUFSIZE 15


#define  C_BAUDRATE	57600
#define  D_BAUDRATE	57600

#memmap xmem
#use "dcrtcp.lib"

udp_Socket interface_sock;
udp_Socket field_sock;

struct interface_struct {
	unsigned char axis[12];
   unsigned char btn[32];

} interface;

struct motor_struct {
	unsigned char trex[2];
   unsigned char trex_prev[2];
   unsigned char trexCycle;
}	motors;

//motors.trex[0] = 184;
//motors.x2[0] = 49;

void setTREX0(unsigned char spd){
		if(spd < 127){
		  serDputc(0xC1);
        serDputc((127-spd));
      }
      else if(spd >127){
			serDputc(0xC2);
         serDputc(127 - (255 - spd));
      }
      else{
      //Possible Braking
			serDputc(0xC1);
         serDputc(0);
      }
      return;
}

void setTREX1(unsigned char spd){
		if(spd < 127){
		  serDputc(0xC9);
        serDputc((127-spd));
      }
      else if(spd > 127){
			serDputc(0xCA);
         serDputc(127 - (255 - spd));
      }
      else{
      //Possible Braking
			serDputc(0xC9);
         serDputc(0);
      }
      return;
}

int check_socket(void)
{
	static char buf[128];
   int c;
   auto int 	length;


	#GLOBAL_INIT
	{
		memset(buf, 0, sizeof(buf));
	}


	if (-1 == udp_recv(&interface_sock, buf, sizeof(buf))) {
	//no packet
		return 0;
	}


   //Temporary UDP punchthru garbage packets for stubborn routers
   length = strlen(buf) + 1;
   if (buf[0] == 'U' && buf[1] == 'D'){
   printf("UDP Punchthru Received");
   	udp_send(&interface_sock, buf, length);
   	udp_send(&interface_sock, buf, length);
      udp_send(&interface_sock, buf, length);
   }
	length = strlen(buf) + 1;
//                      printf("debug: %s", buf);


   //INTERFACE PACKET
   if (buf[0] == 'X'){

		udp_send(&interface_sock, buf, 18);

      interface.axis[0] = buf[1];
      interface.axis[1] = buf[2];
      interface.axis[2] = buf[3];
      interface.axis[3] = buf[4];
      interface.axis[4] = buf[5];
      interface.axis[5] = buf[6];
      interface.axis[6] = buf[7];
      interface.axis[7] = buf[8];
      interface.axis[8] = buf[9];
      interface.axis[9] = buf[10];
      interface.axis[10] = buf[11];
      interface.axis[11] = buf[12];
      //Byte 0
      interface.btn[0] = buf[13] & 0x01;
      interface.btn[1] = buf[13] & 0x02;
      interface.btn[2] = buf[13] & 0x04;
      interface.btn[3] = buf[13] & 0x08;
      interface.btn[4] = buf[13] & 0x10;
      interface.btn[5] = buf[13] & 0x20;
      interface.btn[6] = buf[13] & 0x40;
      interface.btn[7] = buf[13] & 0x80;

   	//Byte 1
      interface.btn[8] = buf[14] & 0x01;
      interface.btn[9] = buf[14] & 0x02;
      interface.btn[10] = buf[14] & 0x04;
      interface.btn[11] = buf[14] & 0x08;
      interface.btn[12] = buf[14] & 0x10;
      interface.btn[13] = buf[14] & 0x20;
      interface.btn[14] = buf[14] & 0x40;
      interface.btn[15] = buf[14] & 0x80;
   	//Byte 2
      interface.btn[16] = buf[15] & 0x01;
      interface.btn[17] = buf[15] & 0x02;
      interface.btn[18] = buf[15] & 0x04;
      interface.btn[19] = buf[15] & 0x08;
      interface.btn[20] = buf[15] & 0x10;
      interface.btn[21] = buf[15] & 0x20;
      interface.btn[22] = buf[15] & 0x40;
      interface.btn[23] = buf[15] & 0x80;
   	//Byte 3
      interface.btn[24] = buf[16] & 0x01;
      interface.btn[25] = buf[16] & 0x02;
      interface.btn[26] = buf[16] & 0x04;
      interface.btn[27] = buf[16] & 0x08;
      interface.btn[28] = buf[16] & 0x10;
      interface.btn[29] = buf[16] & 0x20;
      interface.btn[30] = buf[16] & 0x40;
      interface.btn[31] = buf[16] & 0x80;





	}
	return 1;
}

//TReX Update pings
void trex_update(){
   unsigned char spd;
   unsigned char spd_prev;

	if(serDwrUsed() == 0){
   /*
		motors.trexCycle++;
      if(motors.trexCycle>1) motors.trexCycle = 0;
		switch(motors.trexCycle){
	    case 0 : setTREX0(motors.trex[0]);
	    case 1 : setTREX1(motors.trex[1]);
	   }
   */

	if(motors.trex[0] != motors.trex_prev[0]){
//	   setTREX0(motors.trex[0]);
		spd = motors.trex[0];
		spd_prev = motors.trex_prev[0];

		if(spd < 127){
		  serDputc(0xC1);
        serDputc((127-spd));
      }
      else if(spd >127){
			serDputc(0xC2);
         serDputc(127 - (255 - spd));
      }
      else{
      //Possible Braking
			if(spd_prev < 127){
				serDputc(0xC1);
         }
         else{
				serDputc(0xC2);
         }
         serDputc(0);
      }
      motors.trex_prev[0] = motors.trex[0];
   }



  	if(motors.trex[1] != motors.trex_prev[1]){
//	   setTREX0(motors.trex[1]);
		spd = motors.trex[1];
		spd_prev = motors.trex_prev[1];

		if(spd < 127){
		  serDputc(0xC9);
        serDputc((127-spd));
      }
      else if(spd >127){
			serDputc(0xCA);
         serDputc(127 - (255 - spd));
      }
      else{
      //Possible Braking
			if(spd_prev < 127){
				serDputc(0xC9);
         }
         else{
				serDputc(0xCA);
         }
         serDputc(0);
      }
      motors.trex_prev[1] = motors.trex[1];
   }

   }
}

//FAST USER CODE HERE AKA AUTONOMOUS
void user_code_fast(){
}

//USER CODE HERE     RUNS WHEN A UDP PACKET IS RECEIVED
void packet_recv(){

/*
		//send to X2
      if(interface.axis[1] == 255){interface.axis[1]=254;}
      if(interface.axis[3] == 255){interface.axis[3]=254;}
      serCputc(255);
		serCputc(interface.axis[1]);
		serCputc(interface.axis[3]);
  */
		motors.trex[0] = interface.axis[1];
      motors.trex[1] = interface.axis[3];
      //Left X axis
//		setTREX1(interface.axis[1]);
      //Right X axis
//		setTREX2(interface.axis[3]);
}


void main()
{
   unsigned char ip[16];
	int i;

   serCopen(C_BAUDRATE);
   serCwrFlush();
   serCrdFlush();

	serDopen(D_BAUDRATE);
   serDwrFlush();
   serDrdFlush();

   serCputs("RCM5600W-Init\r\n");

//init network
	sock_init_or_exit(1);

	if(!udp_open(&interface_sock, INTERFACE_PORT, resolve(REMOTE_IP), 0, NULL)) {
		printf("interface port failed!\n");
		exit(0);
	}

   if(!udp_open(&field_sock, FIELD_PORT, resolve(FIELD_IP), 0, NULL)) {
		printf("field port failed!\n");
		exit(0);
	}

   printf("NET-Init\r\n");
      //Print IP to DynamicC
	printf("IP :: %s \r\n", inet_ntoa(ip, _if_tab[IF_DEFAULT].ipaddr));
      //Print IP to X2 LCD
	serCputs(inet_ntoa(ip, _if_tab[IF_DEFAULT].ipaddr));


   //default interface values
   for(i=0;i<12;i++){
   	interface.axis[i] = 127;
   }

   //zero out motors
   for(i=0;i<2;i++){
   	motors.trex[i] = 127;
   }
   //Main Loop
	for(;;) {
		trex_update();
      user_code_fast();

		tcp_tick(NULL);
		if(check_socket()){
      //If packet is received
      packet_recv();
      }
	}
}



